// File:		cmptCMD.cpp
// Version:		1.0b
// Written by:	ChinJohn Wang (cjwang@esit.com)
// Date:		Jan 17, 2011
// Description: This code is designed to work with the node manager and JAUS library software

#include "cmptCMD.h"	// USER: Implement and rename this header file. Include prototypes for all public functions contained in this file.

//global variables
char	Def_Address[MAX_ADDRESS_SIZE];		//defualt lif address

//global variables
extern int writeData (wavelengthData lifData);


OjCmpt ocuCreate() 
{
	OjCmpt cmptOCU;

	cmptOCU = ojCmptCreate("lif_OCU", JAUS_SUBSYSTEM_COMMANDER, siOCU_FRAME_RATE_HZ);
	if (cmptOCU == NULL)
	{
		//Exit program
		printf("Error Create Component of siOCU. \n");
		return NULL;
	}


	ojCmptAddService(cmptOCU, JAUS_SUBSYSTEM_COMMANDER);

	ojCmptAddServiceInputMessage(cmptOCU, JAUS_SUBSYSTEM_COMMANDER, JAUS_REPORT_IMAGE, 0xFF);
	ojCmptAddServiceInputMessage(cmptOCU, JAUS_SUBSYSTEM_COMMANDER, JAUS_QUERY_COMPONENT_STATUS, 0xFF);

	ojCmptAddServiceOutputMessage(cmptOCU, JAUS_SUBSYSTEM_COMMANDER, JAUS_QUERY_IMAGE, 0xFF);
	ojCmptAddServiceOutputMessage(cmptOCU, JAUS_SUBSYSTEM_COMMANDER, JAUS_QUERY_COMPONENT_STATUS, 0xFF);

	
	ojCmptSetStateCallback(cmptOCU, JAUS_INITIALIZE_STATE, ocuStateProc);
	ojCmptSetStateCallback(cmptOCU, JAUS_READY_STATE, ocuStateProc);
	//ojCmptSetMessageProcessorCallback(cmptOCU,ocuReportImageMessageCallback);
	ojCmptSetMessageCallback(cmptOCU, JAUS_REPORT_IMAGE, ocuReportImageMessageCallback);
	ojCmptSetMessageCallback(cmptOCU, JAUS_REPORT_COMPONENT_STATUS, ocuReportStatusMessageCallback);
	ojCmptAddSupportedSc(cmptOCU, JAUS_REPORT_IMAGE);

	ojCmptSetUserData(cmptOCU, ocuDataCreate());

	ojCmptSetState(cmptOCU, JAUS_INITIALIZE_STATE);

	if(ojCmptRun(cmptOCU))
	{
		ojCmptDestroy(cmptOCU);
		printf("Error Starting Component of Science Instrument. \n");
		return NULL;
	}


	return cmptOCU;
}

void ocuStateProc(OjCmpt ocu)
{
	ocuData data = (ocuData) ojCmptGetUserData(ocu);	
	bool prelive = data->liveLIF;
	
	if (data->queryImage)
	{
		//monitor data received
		if (data->queryTimeOut < time(NULL)) 
		{
			data->queryImage = false;  //stop waiting
			printf("\nError: Time out of data waiting. \n");
		}
		
		
		if (data->dataReady)
		{
			data->queryImage = false;  //stop waiting
			writeData(data->wlData);
			data->dataReady = false;		//clear flag
		}
	}
	else
	{
		//monitoring LIF status
		if ( (time(NULL)- data->lastActive) > 2)
		{
			//lose the connect
			data->liveLIF = false;
			data->lifState = JAUS_INVALID_STATE;
			ojCmptSetState(ocu, JAUS_INITIALIZE_STATE);
			//ojCmptSetUserData(ocu, data);	//save data
		}
			if ( (prelive == true) && (data->liveLIF == false) )printf("\nlif out timed. \n");
			requirLIFstatus(ocu);
		}
}

void ocuReportStatusMessageCallback(OjCmpt ocu, JausMessage report)
{
	ocuData data = (ocuData) ojCmptGetUserData(ocu);
	ReportComponentStatusMessage message = reportComponentStatusMessageFromJausMessage(report);

	//printf("message received from=%i , component=%i , type=%X\n", report->source->subsystem,report->source->component, report->commandCode);
	if ( message != NULL )
	{
		data->lifAddress = jausAddressClone(message->source);
		data->liveLIF = true;
		data->lifState = message->primaryStatusCode;
		data->lastActive = time(NULL);
		//ojCmptSetUserData(ocu, data); //save it
		printf("."); 
	}
}

void ocuReportImageMessageCallback(OjCmpt ocu, JausMessage report)
{
	printf("message received from=%i , component=%i , type=%X\n", report->source->subsystem, report->source->component, report->commandCode);
	ocuData data = (ocuData) ojCmptGetUserData(ocu);

	data->dataReady = true;
	//data->wlData.wavelength = *report->data ;
	data->wlData.ID = time(NULL);

	printf("\nReceived data virified is ok!\n");
}

void	requirLIFstatus(OjCmpt ocu)
{	
	QueryComponentStatusMessage ocuMsg = queryComponentStatusMessageCreate();
	ocuData data =(ocuData)ojCmptGetUserData(ocu);	
	if(ocuMsg == NULL || data == NULL)
 	{
		return;
	}

	ocuMsg->destination = jausAddressClone(data->lifAddress);
	ocuMsg->source = ojCmptGetAddress(ocu);

	// Send the requery Message
	ojCmptSendMessage(ocu, queryComponentStatusMessageToJausMessage(ocuMsg) );

	// Destroy the Messages
	queryComponentStatusMessageDestroy(ocuMsg);

	//printf("query LIF Status Message send out. \n");
}

void	queryLIFdata(OjCmpt ocu)
{
	printf("\n\nQuery message had been sent out.\n\n Waiting data ....\n\n");

	ocuData data =(ocuData)ojCmptGetUserData(ocu);	
	if(data == NULL) return;

	JausMessage jausMessage = jausMessageCreate();
	jausMessage->destination = jausAddressClone(data->lifAddress);
	jausMessage->source = ojCmptGetAddress(ocu);  //pointer is cloned
	jausMessage->commandCode = JAUS_QUERY_IMAGE;

	// Send the requery Message
	ojCmptSendMessage(ocu, jausMessage );

	// Destroy the Messages
	jausMessageDestroy(jausMessage);
}

ocuData ocuDataCreate(void)
{
	ocuData data = (ocuData)malloc( sizeof(ocuDataStruct) );
	if(data == NULL)
	{
		return NULL;
	}

	JausUnsignedShort zero[LIF_WAVELENGTH_SIZE]={0};

	data->wlData.ID = 0;
	memcpy(data->wlData.wavelength, zero, sizeof(zero)); // init wavelength
	data->lifAddress = StringtoJausAddr(Def_Address);
	data->liveLIF = false;
	data->queryImage = false;
	data->dataReady = false;
	data->lifState =JAUS_UNKNOWN_STATE;
	data->lastActive = time(NULL);
	data->queryTimeOut  = time(NULL);

	return data;
}


JausAddress StringtoJausAddr(const char str[MAX_ADDRESS_SIZE])
{
	JausAddress address = jausAddressCreate();
	int ip[4]={0,0,0,0};
	int count;

	count = sscanf_s( str, "%d.%d.%d.%d", &ip[0],&ip[1],&ip[2],&ip[3]);
	printf("Jaus Address = %i.%i.%i.%i \n",ip[0],ip[1],ip[2],ip[3]);
	if (count != 4) return NULL;  //error
	address->subsystem = ip[0];
	address->node = ip[1];
	address->component = ip[2];
	address->instance = ip[3];

	if (jausAddressIsValid(address)) 
	{
		return address;
	}
	else
	{
		return NULL;
	}
}